#include <rtthread.h>
#include <rthw.h>
#include <misc.h>
#include "./modbus_slave_core.h"
#include "../../parameter.h"
#include "../Interpolation.hpp"
#include "../../Math/Bezier.hpp"
#include "../Coordinate_Velocity.hpp"
#include "../Location.h"
#include "../Move_Control.h"
#include <arm_math.h>
#include "../../Math/My_Math.hpp"
#include "../../Drivers/Mecanum_AGV.hpp"
#include "../Avoid.h"

#define LOG_TAG "modbus slave core" /* 该模块TAG */
#define LOG_LVL LOG_LVL_DBG         /* 静态过滤级别为调试级 */
#include <ulog.h>                   /* 必须放在宏定义下面 */

/* 寄存器定义,其中浮点数采用采用IEEE 754 标准，且内存分布为ABCD */
enum Register_Addr
{
    /* 运动控制寄存器 */
    Init_Flag_Reg = 0x0000, /* 运动模块初始化标志 */
    Timeout_Reg,            /* 通信超时时间(ms) */
    Soft_Restart_Reg,       /* 软件复位 */
    Wrong_Code_LOW_Reg,     /* 错误代码低半字 */
    Wrong_Code_HIGH_Reg,    /* 错误代码高半字 */
    Move_State_Reg,         /* 运动状态 */

    /* 车辆状态寄存器 */
    Data_Matrix_Reg,      /* 二维码状态 */
    Data_Matrix_Low_Reg,  /* 二维码TAG号低半字 */
    Data_Matrix_High_Reg, /* 二维码TAG号高半字 */

    Motor_State_reg, /* 电机使能状态寄存器 */
    PBS_Area_reg,    /* PBS传感器检测区域 */
    Avoid_Check_Reg, /* 避障传感器检测 */
    Reset_Theta_Reg, /* 重置角度数据 */

    Actual_X_Velocity_LOW_HW_Reg = 0x0020, /* 车辆实际运行x轴线速度,浮点数低半字，即bit[0..15],单位为mm/s */
    Actual_X_Velocity_HIGH_HW_Reg,         /* 车辆实际运行x轴线速度,浮点数高半字，即bit[16..31],单位为mm/s */
    Actual_Y_Velocity_LOW_HW_Reg,          /* 车辆实际运行y轴线速度,浮点数低半字，即bit[0..15],单位为mm/s */
    Actual_Y_Velocity_HIGH_HW_Reg,         /* 车辆实际运行y轴线速度,浮点数高半字，即bit[16..31],单位为mm/s */
    Actual_Angular_Velocity_LOW_HW_Reg,    /* 车辆实际运行角速度,浮点数低半字，即bit[0..15],逆时针正,单位为°/s */
    Actual_Angular_Velocity_HIGH_HW_Reg,   /* 车辆实际运行角速度,浮点数高半字，即bit[16..31],逆时针正,单位为°/s */

    Actual_X_Coor_LOW_HW_Reg,  /* 车辆实际x轴坐标,浮点数低半字，即bit[0..15],单位为mm */
    Actual_X_Coor_HIGH_HW_Reg, /* 车辆实际x轴坐标,浮点数高半字，即bit[16..31],单位为mm */
    Actual_Y_Coor_LOW_HW_Reg,  /* 车辆实际y轴坐标,浮点数低半字，即bit[0..15],单位为mm  */
    Actual_Y_Coor_HIGH_HW_Reg, /* 车辆实际y轴坐标,浮点数高半字，即bit[16..31],单位为mm */
    Actual_Theta_LOW_HW_Reg,   /* 车辆实际角度,浮点数低半字，即bit[0..15],逆时针正,单位为° */
    Actual_Theta_HIGH_HW_Reg,  /* 车辆实际角度,浮点数高半字，即bit[16..31],逆时针正,单位为° */

    /* 位置控制寄存器，输入浮点数采用IEEE 754 标准 */
    X_Coor_Low_HW_Reg = 0x0030, /* x轴坐标，浮点数低半字，即bit[0..15],单位为mm */
    X_Coor_High_HW_Reg,         /* x轴坐标，浮点数高半字，即bit[16..31],单位为mm */
    Y_Coor_Low_HW_Reg,          /* y轴坐标,浮点数低半字，即bit[0..15],单位为mm */
    Y_Coor_High_HW_Reg,         /* y轴坐标,浮点数高半字，即bit[16..31],单位为mm */
    Theta_Low_HW_Reg,           /* 角度,浮点数低半字，即bit[0..15],单位为° */
    Theta_High_HW_Reg,          /* 角度,浮点数高半字，即bit[16..31],单位为° */

    Threshold_Reg = 0x0040, /* 插补阈值寄存器(mm) */
    Max_Velocity_Reg,       /* 运行时最大速度(mm/s) */
    Velocity_Acc_Reg,       /* 加速度,从0速到运行时最大速度的时间(s) */
    Velocity_Dec_Reg,       /* 减速度,从运行时最大速度到0速的时间(s) */
    Slow_Time_Reg,          /* 低速运行时间(s) */

    Demo_Reg = 0x0050, /* 测试寄存器 */
};

union Modbus_Float {
    float fdata; /* 浮点数 */
    struct
    {
        short sdata_high; /* 大小端，先高字节 */
        short sdata_low;
    };
    char cdata[4];
} ALIGN(RT_ALIGN_SIZE); /* 定义了Modbus发送接收数据用的浮点数类型 */

/* 保存接收数据中的Modbus关键字 */
static char func_code = 0;       /* 功能码 */
static short start_addr = 0;     /* 起始地址 */
static short reg_num = 0;        /* 寄存器数量 */
static const char *reg_data = 0; /* 数据 */

static void Read(char *buf, const short data) RT_UNUSED;
static void Read(char *buf, const bool data) RT_UNUSED;
static void Read(char *buf, unsigned short data) RT_UNUSED;
static void Read(char *buf, int data) RT_UNUSED;

static void Write(const char *buf, short *reg) RT_UNUSED;
static void Write(const char *buf, bool *reg) RT_UNUSED;
static void Write(const char *buf, unsigned short *reg) RT_UNUSED;

static void Read_Reg(char *data, int reg_add) RT_USED;        /* 将寄存器数据读到data中 */
static void Write_Reg(const char *data, int reg_add) RT_USED; /* 将data数据写入到寄存器中 */

static struct rt_mutex modbus_mutex; /* 定义临界区 */
/* 寄存器数据 */
static short init_flag = 0;                 /* 设备初始化标志 */
static bool soft_restart_flag = false;      /* 软件复位标志 */
static bool set_destination_flag = false;   /* true表示设置了终点 */
static bool set_interpolation_flag = false; /* true表示设置了插补参数,即点输入完毕 */

static bool data_matrix_flag = false; /* true表示读取到二维码 */
static int data_matrix_data = 0;      /* 二维码TAG号 */

static bool avoid_check = false;

static union Modbus_Float set_x, set_y, set_theta; /* 上位机设置目标点坐标 */
static union Wrong_Code_Union wrong_code;          /* 错误代码 */

static struct Interpolation_Class::Interpolation_Parameter_Struct interpolation_parameter; /* 插补参数 */

static Bezier_Curve_Class *origin_bezier_curve = nullptr;                    /* 指向第一段Bezier曲线 */
static Bezier_Curve_Class *current_bezier_curve = nullptr;                   /* 指向当前一段Bezier曲线 */
static Bezier_Curve_Class *Malloc_Bezier(Bezier_Curve_Class *pre = nullptr); /* 申请并初始化一块Bezier曲线空间 */
static void Save_Bezier_Destination(void);                                   /* 保存目标点 */

static bool update_actual_coor_flag = false, update_actual_velocity_flag = false; /* 实际坐标,实际速度已更新标志 */
static union Modbus_Float actual_coor[3], actual_velocity[3];                     /* 实际坐标,实际速度 */
static void Update_Actual_Coor(void);                                             /* 更新实际坐标 */
static void Update_Actual_Velocity(void);                                         /* 更新实际速度 */

static struct rt_timer timeout_tim;            /* 设备通信超时检测定时器 */
static void TimeOut_CallBack(void *parameter); /* 定时器超时函数 */
static unsigned short timeout_cnt = 0;         /* 通信超时时间 */

static void Modbus_Service_Func(void);               /* Modbus服务函数，用于执行动作 */
static void Free_Bezier(Bezier_Curve_Class *origin); /* 释放Bezier曲线占用的内存 */

static bool task_start_flag = false;

static unsigned short demo_value = 0;

/**
 * @description: 根据Modbus的PDU操作相应寄存器
 * @param {type} 
 * @return: >0 应答数据长度
 *          0  不支持该功能码
 *          -1 数据长度错误
 */
int Modbus_Slave_Core(const char function_code, /* 功能码 */
                      const char *rx_data,      /* 数据 */
                      const int rx_data_length, /* 数据长度 */
                      char *tx_data             /* 应答数据 */
)
{
    int tx_length = 0;
    rt_mutex_take(&modbus_mutex, RT_WAITING_FOREVER);

    /* 重置超时检测定时器 */
    if (timeout_cnt > 0)
    {
        rt_timer_stop(&timeout_tim);  /* 关闭定时器 */
        rt_timer_start(&timeout_tim); /* 启动定时器 */
    }

    func_code = function_code;
    switch (func_code)
    {
    /*	读多个寄存器	0x03
	*	功能码	0x03
	*	寄存器起始地址(高字节)
	*	寄存器起始地址(低字节)
	*	寄存器数量(高字节)
	*	寄存器数量(低字节)
	*/
    case 0x03:
    {
        if (rx_data_length < 4)
        {
            tx_length = -1;
            break;
        }

        start_addr = (rx_data[0] << 8) | rx_data[1];
        reg_num = (rx_data[2] << 8) | rx_data[3];

        tx_data[0] = func_code;
        tx_data[1] = reg_num * 2; /* 字节数 */
        tx_data += 2;
        tx_length = 2;
        while (reg_num)
        {
            Read_Reg(tx_data, start_addr);
            start_addr++;
            reg_num--;
            tx_length += 2;
            tx_data += 2;
        }
    }
    break;
    /*	写单个寄存器	0x06
	*	功能码	0x06
	*	寄存器地址(高字节)
	*	寄存器地址(低字节)
	*	寄存器值(高字节)
	*	寄存器值(低字节)
	*/
    case 0x06:
    {
        if (rx_data_length < 4)
        {
            tx_length = -1;
            break;
        }
        start_addr = (rx_data[0] << 8) | rx_data[1];
        reg_data = &rx_data[2];

        Write_Reg(reg_data, start_addr);
        tx_data[0] = func_code;
        rt_memcpy((tx_data + 1), rx_data, 4);
        tx_length = 5;
    }
    break;
    /*	写多个寄存器	0x10
	*	功能码	0x10
	*	寄存器起始地址(高字节)
	*	寄存器起始地址(低字节)
	*	寄存器数量(高字节)
	*	寄存器数量(低字节)
	*	数据域余下字节数(不含校验)
	*	寄存器值(高字节)
	*	寄存器值(低字节)
	*/
    case 0x10:
    {
        if (rx_data_length < (rx_data[4] + 5))
        {
            tx_length = -1;
            LOG_I("数据长度错误");
            break;
        }
        start_addr = (rx_data[0] << 8) | rx_data[1];
        reg_num = (rx_data[2] << 8) | rx_data[3];
        reg_data = &rx_data[5];

        while (reg_num)
        {
            Write_Reg(reg_data, start_addr);
            start_addr++;
            reg_data += 2;
            reg_num--;
        }

        tx_data[0] = func_code;
        rt_memcpy((tx_data + 1), rx_data, 4);
        tx_length = 5;
    }
    break;
    default:
    {
        LOG_I("cmd error.");
        tx_length = 0;
    }
    break;
    }

    Modbus_Service_Func(); /* 执行服务函数 */

    rt_mutex_release(&modbus_mutex);
    return tx_length;
}

/**
 * @description: 设置初始化标志
 * @param {type}
 * @return:
 */
void Set_Init_Flag(enum Init_Event_Enum flag)
{
    rt_base_t temp;
    temp = rt_hw_interrupt_disable();
    init_flag |= (1 << (int)flag); /* 设置标志位 */
    rt_hw_interrupt_enable(temp);
}
short Return_Init_Flag(void)
{
    return init_flag;
}

/**
 * @description: 设置二维码TAG号
 * @param {type}
 * @return:
 */
void Set_Data_Matrix_Flag(char read_flag, unsigned int num)
{
    if (read_flag)
    {
        data_matrix_flag = true;
        data_matrix_data = num;
    }
    else
    {
        data_matrix_flag = false;
    }
}
int Read_Data_Matrix(void)
{
    return data_matrix_flag ? data_matrix_data : -1;
}

inline void Read(char *buf, const short data)
{
    *buf = data >> 8;
    buf++;
    *buf = data & 0x00FF;
}

inline void Read(char *buf, const bool data)
{
    short temp = data ? 1 : 0;
    Read(buf, temp);
}

inline void Read(char *buf, unsigned short data)
{
    Read(buf, (short)data);
}

inline void Read(char *buf, int data)
{
    Read(buf, (short)data);
}

inline void Write(const char *buf, short *reg)
{
    *reg = (buf[0] << 8) | (buf[1]);
}

inline void Write(const char *buf, bool *reg)
{
    short temp = 0;
    Write(buf, &temp);
    *reg = temp ? true : false;
}

inline void Write(const char *buf, unsigned short *reg)
{
    Write(buf, (short *)reg);
}

void Read_Reg(char *data, int reg_add)
{
    /* 修改寄存器 */
    enum Register_Addr addr = (enum Register_Addr)reg_add;
    switch (addr)
    {
    case Init_Flag_Reg:
        Read(data, init_flag);
        break;
    case Wrong_Code_LOW_Reg:
        Read(data, (unsigned short)(wrong_code.wrong_code & 0x0000FFFF));
        break;
    case Wrong_Code_HIGH_Reg:
        Read(data, (unsigned short)(wrong_code.wrong_code >> 16));
        break;
    case Move_State_Reg:
    {
        enum AGV_Move_State_Enum temp = Read_AGV_Move_State(); /* 获取车辆状态 */
        Read(data, (int)temp);
    }
    break;
    case Data_Matrix_Reg:
        Read(data, data_matrix_flag);
        break;
    case Data_Matrix_Low_Reg:
        Read(data, (unsigned short)(data_matrix_data & 0x0000FFFF));
        break;
    case Data_Matrix_High_Reg:
        Read(data, (unsigned short)(data_matrix_data >> 16));
        break;
    case Motor_State_reg:
    {
        unsigned short temp = Mecanum_AGV_Class::Read_Motor_Mode(); /* 读取电机状态 */
        Read(data, temp);
    }
    break;
    case PBS_Area_reg:
    {
        unsigned short temp = Return_Avoid_Area();
        Read(data, temp);
    }
    break;
    case Avoid_Check_Reg:
    {
        Read(data, avoid_check);
    }
    break;
    case Actual_X_Velocity_LOW_HW_Reg:
        Update_Actual_Velocity();
        Read(data, actual_velocity[0].sdata_low);
        break;
    case Actual_X_Velocity_HIGH_HW_Reg:
        Update_Actual_Velocity();
        Read(data, actual_velocity[0].sdata_high);
        break;
    case Actual_Y_Velocity_LOW_HW_Reg:
        Update_Actual_Velocity();
        Read(data, actual_velocity[1].sdata_low);
        break;
    case Actual_Y_Velocity_HIGH_HW_Reg:
        Update_Actual_Velocity();
        Read(data, actual_velocity[1].sdata_high);
        break;
    case Actual_Angular_Velocity_LOW_HW_Reg:
        Update_Actual_Velocity();
        Read(data, actual_velocity[2].sdata_low);
        break;
    case Actual_Angular_Velocity_HIGH_HW_Reg:
        Update_Actual_Velocity();
        Read(data, actual_velocity[2].sdata_high);
        break;
    case Actual_X_Coor_LOW_HW_Reg:
        Update_Actual_Coor();
        Read(data, actual_coor[0].sdata_low);
        break;
    case Actual_X_Coor_HIGH_HW_Reg:
        Update_Actual_Coor();
        Read(data, actual_coor[0].sdata_high);
        break;
    case Actual_Y_Coor_LOW_HW_Reg:
        Update_Actual_Coor();
        Read(data, actual_coor[1].sdata_low);
        break;
    case Actual_Y_Coor_HIGH_HW_Reg:
        Update_Actual_Coor();
        Read(data, actual_coor[1].sdata_high);
        break;
    case Actual_Theta_LOW_HW_Reg:
        Update_Actual_Coor();
        Read(data, actual_coor[2].sdata_low);
        break;
    case Actual_Theta_HIGH_HW_Reg:
        Update_Actual_Coor();
        Read(data, actual_coor[2].sdata_high);
        break;
    default:
        break;
    }
}

void Write_Reg(const char *data, int reg_add)
{
    /* 修改寄存器 */
    enum Register_Addr addr = (enum Register_Addr)reg_add;
    switch (addr)
    {
    case Timeout_Reg:
    {
        Write(data, &timeout_cnt); /* 保存待修改时间 */
        if (0 == timeout_cnt)
        {
            rt_timer_stop(&timeout_tim);            /* 关闭定时器 */
            wrong_code.timeout_no_set_wrong = true; /* 设置错误标志 */
        }
        else
        {
            rt_tick_t temp = timeout_cnt;
            LOG_I("set timeout: %ldms.", temp);                            /* 输出设置的通信超时时间 */
            rt_timer_stop(&timeout_tim);                                   /* 关闭定时器 */
            rt_timer_control(&timeout_tim, RT_TIMER_CTRL_SET_TIME, &temp); /* 设置单次超时时间 */
            rt_timer_start(&timeout_tim);                                  /* 启动定时器 */

            wrong_code.timeout_no_set_wrong = false; /* 清除错误标志 */
        }
    }
    break;
    case Soft_Restart_Reg:
        Write(data, &soft_restart_flag);
        break;
    case Move_State_Reg:
    {
        short temp;
        Write(data, &temp);
        if ((temp >= Move_Control_Continue) && (temp <= Move_Control_Cancel))
            Write_Move_Control_Event((enum Move_Control_Event_Enum)temp); /* 运动控制 */
        else if (Move_Start == temp)
        {
            task_start_flag = true;
        }
        else if ((Move_Start == temp) && (origin_bezier_curve != nullptr))
            Free_Bezier(origin_bezier_curve); /* 释放内存空间 */
    }
    break;
    case Motor_State_reg:
    {
        unsigned short temp;
        Write(data, &temp);
        Mecanum_AGV_Class::Set_Motor_Mode((Motor_Class::Motor_State_Enum)temp);
    }
    break;
    case PBS_Area_reg:
    {
        unsigned short temp;
        Write(data, &temp);
        Avoid_Set_Area(temp);
    }
    break;
    case Reset_Theta_Reg:
    {
        unsigned short temp;
        Write(data, &temp);
        if (temp)
        {
            Reset_Angle();         /* 将角度缩放至(-180,+180] */
            Reset_Virtual_Angle(); /* 缩放虚拟角度 */
        }
    }
    break;
    case X_Coor_Low_HW_Reg:
        Write(data, &set_x.sdata_low);
        set_destination_flag = true;
        break;
    case X_Coor_High_HW_Reg:
        Write(data, &set_x.sdata_high);
        set_destination_flag = true;
        break;
    case Y_Coor_Low_HW_Reg:
        Write(data, &set_y.sdata_low);
        set_destination_flag = true;
        break;
    case Y_Coor_High_HW_Reg:
        Write(data, &set_y.sdata_high);
        set_destination_flag = true;
        break;
    case Theta_Low_HW_Reg:
        Write(data, &set_theta.sdata_low);
        set_destination_flag = true;
        break;
    case Theta_High_HW_Reg:
        Write(data, &set_theta.sdata_high);
        set_destination_flag = true;
        break;
    case Threshold_Reg:
    {
        short temp = 0;
        Write(data, &temp);
        interpolation_parameter.threshold = temp;
        set_interpolation_flag = true;
    }
    break;
    case Max_Velocity_Reg:
    {
        short temp = 0;
        Write(data, &temp);
        interpolation_parameter.max_velocity = temp;
        if (interpolation_parameter.max_velocity > agv_parameter->max_velocity_line)
        {
            interpolation_parameter.max_velocity = agv_parameter->max_velocity_line;
        }
        set_interpolation_flag = true;
    }
    break;
    case Velocity_Acc_Reg:
    {
        short temp = 0;
        Write(data, &temp);
        interpolation_parameter.acc = temp;
        if (interpolation_parameter.acc < agv_parameter->default_acc)
        {
            interpolation_parameter.acc = agv_parameter->default_acc;
        }
        interpolation_parameter.acc = (interpolation_parameter.max_velocity) / interpolation_parameter.acc;
        set_interpolation_flag = true;
    }
    break;
    case Velocity_Dec_Reg:
    {
        short temp = 0;
        Write(data, &temp);
        interpolation_parameter.dec = temp;
        if (interpolation_parameter.dec < agv_parameter->default_dec)
        {
            interpolation_parameter.dec = agv_parameter->default_dec;
        }
        interpolation_parameter.dec = (interpolation_parameter.max_velocity) / interpolation_parameter.dec;

        set_interpolation_flag = true;
    }
    break;
    case Slow_Time_Reg:
    {
        short temp = 0;
        Write(data, &temp);
        interpolation_parameter.slow_time = (float)temp;
        set_interpolation_flag = true;
    }
    break;
    case Demo_Reg:
        Write(data, &demo_value);
        break;
    default:
        break;
    }
}

void Modbus_Slave_Core_Init(void)
{
    /* 初始化用于通信超时检测的定时器 */
    rt_timer_init(&timeout_tim, "timeout time", TimeOut_CallBack, RT_NULL, 0, RT_TIMER_FLAG_ONE_SHOT | RT_TIMER_FLAG_SOFT_TIMER);
    rt_mutex_init(&modbus_mutex, "modbus mutex", RT_IPC_FLAG_FIFO);
    wrong_code.timeout_no_set_wrong = true;

    interpolation_parameter.threshold = 0.0f;
    interpolation_parameter.min_velocity = agv_parameter->min_velocity_line;
    interpolation_parameter.max_velocity = agv_parameter->max_velocity_line;
    interpolation_parameter.acc = agv_parameter->default_acc;
    interpolation_parameter.dec = agv_parameter->default_dec;
    interpolation_parameter.acc = (interpolation_parameter.max_velocity) / interpolation_parameter.acc;
    interpolation_parameter.dec = (interpolation_parameter.max_velocity) / interpolation_parameter.dec;
    interpolation_parameter.slow_time = 0.0f;
    interpolation_parameter.user_data = RT_NULL;
}

unsigned int Read_Wrong_Code(void)
{
    return wrong_code.wrong_code;
}

void Set_Avoid_Check(char flag)
{
    avoid_check = (bool)flag;
}

Bezier_Curve_Class *Malloc_Bezier(Bezier_Curve_Class *pre)
{
    Bezier_Curve_Class *temp = (Bezier_Curve_Class *)rt_malloc(sizeof(Bezier_Curve_Class));
    if (nullptr == temp)
    {
        wrong_code.sram_full = 1;
    }
    else
    {
        temp->pre_curve = pre;
        temp->next_curve = nullptr;
        temp->P1 = nullptr;
        LOG_I("set destination:%.2f,%.2f,%.2f.", set_x.fdata, set_y.fdata, set_theta.fdata);
        temp->P4.x = set_x.fdata;
        temp->P4.y = set_y.fdata;
        temp->P4.z = set_theta.fdata;
        // temp->P4.z = My_Math_Class::Trans_Rad(set_theta.fdata);
        // temp->P4.z = temp->P4.z * agv_parameter->distance_lx_ly;
    }
    return temp;
}

void Save_Bezier_Destination(void)
{
    if (nullptr == origin_bezier_curve)
    {
        /* 当前还未设置起点 */
        origin_bezier_curve = Malloc_Bezier();      /* 申请第一块空间 */
        current_bezier_curve = origin_bezier_curve; /* 链表当前指针 */
    }
    else
    {
        Bezier_Curve_Class *temp = Malloc_Bezier(current_bezier_curve);
        if (temp)
        {
            /* 申请到了空间 */
            current_bezier_curve->next_curve = temp;
            current_bezier_curve = temp;
        }
    }
}

/**
 * @description: 定时器超时服务函数
 * @param {type}
 * @return:
 */
void TimeOut_CallBack(void *parameter)
{
    LOG_W("Modbus timeout!");                 /* 输出通信超时警告 */
    wrong_code.timeout_wrong = 1;             /* 通信超时 */
    Write_Brake_Event(Communication_Timeout); /* 发送通信超时事件 */
}

void Modbus_Service_Func(void)
{
    update_actual_velocity_flag = false;
    update_actual_coor_flag = false;
    if (soft_restart_flag)
    {
        /* 关闭全局中断，软件复位 */
        soft_restart_flag = false;
        rt_base_t temp = 0;
        temp = rt_hw_interrupt_disable();
        NVIC_SystemReset();
        rt_hw_interrupt_enable(temp);
    }

    if (wrong_code.timeout_wrong)
    {
        LOG_W("Modbus timeout."); /* 通信超时，直接跳过 */
        return;
    }

    if (set_destination_flag)
    {
        set_destination_flag = false;
        Save_Bezier_Destination();
    }
    if (set_interpolation_flag)
    {
        set_interpolation_flag = false;
        LOG_I("v_max:%.2fmm/s,acc:%.2fmm/s^2,dec:%.2fmm/s^2.", interpolation_parameter.max_velocity, interpolation_parameter.acc, interpolation_parameter.dec);
        LOG_I("v_min:%.2fmm/s,s_time:%.2fs,threshold:%.2fmm.", interpolation_parameter.min_velocity, interpolation_parameter.slow_time, interpolation_parameter.threshold);
    }

    switch (demo_value)
    {
    case 1: /* 测试例程1,圆周旋转 */
    {
        float rad = 0.0f;
        for (int i = -90; i <= 270; i += 10)
        {
            rad = (float)i;
            rad = My_Math_Class::Trans_Rad(rad);
            set_x.fdata = 2000.0f + 1500 * arm_cos_f32(rad);
            set_y.fdata = 2000.0f + 1500 * arm_sin_f32(rad);
            set_theta.fdata = 0.0f;
            Save_Bezier_Destination();
        }
        task_start_flag = true;
    }
    break;
    case 2: /* 测试例程2,圆周旋转 */
    {
        float rad = 0.0f;
        for (int i = -90; i <= 270; i += 10)
        {
            rad = (float)i;
            set_theta.fdata = rad;
            rad = My_Math_Class::Trans_Rad(rad);
            set_x.fdata = 2000.0f + 1500 * arm_cos_f32(rad);
            set_y.fdata = 2000.0f + 1500 * arm_sin_f32(rad);
            Save_Bezier_Destination();
        }
        task_start_flag = true;
    }
    break;
    case 3: /* 测试例程3,自定义路径 */
    {
        float rad = 0.0f;
        for (int i = -90; i <= 0; i += 10)
        {
            rad = (float)i;
            set_theta.fdata = rad;
            rad = My_Math_Class::Trans_Rad(rad);
            set_x.fdata = 2000.0f + 1500 * arm_cos_f32(rad);
            set_y.fdata = 1500.0f + 1500 * arm_sin_f32(rad);
            Save_Bezier_Destination();
        }
        for (int i = 0; i <= 180; i += 10)
        {
            rad = (float)i;
            set_theta.fdata = 0.0f;
            rad = My_Math_Class::Trans_Rad(rad);
            set_x.fdata = 2000.0f + 1500 * arm_cos_f32(rad);
            set_y.fdata = 1500.0f + 1500 * arm_sin_f32(rad);
            Save_Bezier_Destination();
        }
        for (int i = 180; i <= 270; i += 10)
        {
            rad = (float)i;
            set_theta.fdata = 180.0f - rad;
            rad = My_Math_Class::Trans_Rad(rad);
            set_x.fdata = 2000.0f + 1500 * arm_cos_f32(rad);
            set_y.fdata = 1500.0f + 1500 * arm_sin_f32(rad);
            Save_Bezier_Destination();
        }
        task_start_flag = true;
    }
    break;
    default:
        break;
    }

    demo_value = 0; /* 数值复位 */

    if (task_start_flag)
    {
        task_start_flag = false;
        if (!origin_bezier_curve)
        {
            LOG_I("no path.");
        }
        else
        {
            /* 指示传感器、驱动器初始化结果 */
            short init_temp = (1 << PGV_Initialization_Completed) | (1 << TL740_Initialization_Completed) | (1 << Motor_Initialization_Completed);
            short init_flag_temp = init_flag; /* 线程安全 */
            if ((init_flag_temp & init_temp) == init_temp)
            {
                interpolation_parameter.user_data = origin_bezier_curve;
                LOG_I("send origin.");
                Set_Destination(&interpolation_parameter); /* 发送地址 */
                origin_bezier_curve = nullptr;
            }
            else
            {
                if (!(init_flag_temp & (1 << PGV_Initialization_Completed)))
                {
                    LOG_I("PGV no inited.");
                }
                if (!(init_flag_temp & (1 << TL740_Initialization_Completed)))
                {
                    LOG_I("TL740 no inited.");
                }
                if (!(init_flag_temp & (1 << Motor_Initialization_Completed)))
                {
                    LOG_I("motor no inited.");
                }
            }
        }
    }
}

void Free_Bezier(Bezier_Curve_Class *origin)
{
    Bezier_Curve_Class *temp = origin;
    if (temp)
    {
        while (temp->next_curve)
        {
            temp = (Bezier_Curve_Class *)temp->next_curve;
            rt_free((Bezier_Curve_Class *)temp->pre_curve);
        }
        rt_free((Bezier_Curve_Class *)temp);
    }
}

void Update_Actual_Coor(void)
{
    if (!update_actual_coor_flag)
    {
        update_actual_coor_flag = true;
        struct Location_Coor_Struct coor;
        Read_Location_Coor(&coor); /* 更新坐标 */
        actual_coor[0].fdata = coor.x_coor;
        actual_coor[1].fdata = coor.y_coor;
        actual_coor[2].fdata = coor.theta;
    }
}

void Update_Actual_Velocity(void)
{
    if (!update_actual_velocity_flag)
    {
        update_actual_velocity_flag = true;
        struct Location_Velocity_Struct v;
        Read_Location_Velocity(&v); /* 更新速度 */

        actual_velocity[0].fdata = v.vx;
        actual_velocity[1].fdata = v.vy;
        actual_velocity[2].fdata = v.w;

        // LOG_D("v:%.2f,%.2f,%.2f", v.vx, v.vy, v.w);
    }
}
